//
// Created by yephiu on 2023/7/11.
//

#include "Nizhou_Security.h"
#include "Ano_FcData.h"
#include "Ano_Imu.h"
#include "Ano_Math.h"
#include "Ano_DT.h"

/**
 * 安全任务
 * @param dT_ms 1ms调度一次
 */
void Nizhou_Security(uint8_t dT_ms)
{
    float pitch=0.0f;
    float roll=0.0f;
    if (flag.unlock_sta==1)
    {
        //TODO:希望继续加入更多的保障措施
        pitch=imu_data.pit;
        roll=imu_data.rol;
        if (ABS(pitch)>30.0f||ABS(roll)>30.0f)
        {
            flag.unlock_cmd=0;
            ANO_DT_SendString("DANGER ALTITUDE! LOCK!");
        }

    }
}